import numpy as np
import math

def euler_to_quaternion(roll, pitch, yaw):
    """
    Converts Euler angles to a quaternion.

    Args:
    - roll: Roll angle in radians
    - pitch: Pitch angle in radians
    - yaw: Yaw angle in radians

    Returns:
    - Quaternion as a numpy array with 4 elements
    """

    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    qw = cy * cp * cr + sy * sp * sr
    qx = cy * cp * sr - sy * sp * cr
    qy = sy * cp * sr + cy * sp * cr
    qz = sy * cp * cr - cy * sp * sr

    return np.array([qw, qx, qy, qz])

def quaternion_to_euler(q):
    """
    Converts a quaternion to Euler angles.

    Args:
    - q: Quaternion as a numpy array with 4 elements

    Returns:
    - Roll, pitch, and yaw angles in radians as a tuple
    """

    # Roll (x-axis rotation)
    sinr_cosp = 2.0 * (q[0] * q[1] + q[2] * q[3])
    cosr_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
    roll = math.atan2(sinr_cosp, cosr_cosp)

    # Pitch (y-axis rotation)
    sinp = 2.0 * (q[0] * q[2] - q[3] * q[1])
    if abs(sinp) >= 1:
        pitch = math.copysign(math.pi / 2, sinp)
    else:
        pitch = math.asin(sinp)

    # Yaw (z-axis rotation)
    siny_cosp = 2.0 * (q[0] * q[3] + q[1] * q[2])
    cosy_cosp = 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])
    yaw = math.atan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

if __name__ == '__main__':
   print(euler_to_quaternion(0,0,1))
   print(quaternion_to_euler([0.87758256,0., 0.,         0.47942554]))